#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <unistd.h>

#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h> 
#include <image_transport/image_transport.h>
#include <sensor_msgs/Image.h>

#include "hk_camera.h"

bool g_exit = false;
void sigint_handler(int sig) {
    printf("捕获到 SIGINT 信号\n");
    printf("处理 SIGINT 信号...\n");
    g_exit = true;
}

int main(int argc, char* argv[])
{
    struct sigaction new_action, old_action;

    // 设置新的信号处理程序
    new_action.sa_handler = sigint_handler;
    sigemptyset(&new_action.sa_mask); // 清空信号屏蔽集
    new_action.sa_flags = 0;

    // 设置SIGINT信号的新处理程序
    if (sigaction(SIGINT, &new_action, &old_action) == -1) {
        perror("无法设置信号处理程序");
        return 1;
    }
    ros::init(argc, argv, "hk_drive_node");
    ros::NodeHandle node;
    image_transport::ImageTransport it(node);
    image_transport::Publisher pub = it.advertise("/cam/rgb_image", 5);

    std::shared_ptr<hk_camera> app_ptr = std::make_shared<hk_camera>();
    app_ptr->regCallBack([&pub](int height, int width, unsigned char* data){
        ros::Time t0 = ros::Time::now();
        cv::Mat srcImage = cv::Mat(height, width, CV_8UC3, data);
        // // 创建一个Mat对象来存储转换后的RGB图像
        // cv::Mat rgbImage;
    
        // // 使用cvtColor函数进行颜色转换
        // cv::cvtColor(srcImage, rgbImage, cv::COLOR_BayerBG2BGR);
        
        //ros
        ros::Time t1 = ros::Time::now();
        sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "rgb8", srcImage).toImageMsg();
        msg->header.stamp = ros::Time::now();
        pub.publish(msg);
        //write
        // cv::imwrite("/home/boot/pic/output_image.png", srcImage);
        ros::Time t2 = ros::Time::now();
        
        ROS_INFO("ptr to mat is: %f", (t1.toSec()- t0.toSec()) * 1000.0);
        ROS_INFO("save: %f", (t2.toSec()- t1.toSec()) * 1000.0);   
    });
    app_ptr->start();  

    ros::Rate loop_rate(10);
    while (ros::ok())
    {   
        if(g_exit)
            break;
        ros::spinOnce();
        loop_rate.sleep();
    }

    app_ptr->stop();
    return 0;
}